#ifndef _CURVEFIT_
#define _CURVEFIT_
#include "B_Form.h"
#include "PP_Form.h"
#include <vector>
#include <eigen3/Eigen/Sparse>
#include <eigen3/Eigen/Dense>
#include <iostream>
using namespace std;

class Point_function : public Function
{
private:
    vector<double> Point, Value, dF, sec_dF;
    int n;
public:
    Point_function(vector<double> _Point, vector<double> _Value, vector<double> _dF, vector<double> _sec_dF): Point(_Point), Value(_Value), dF(_dF), sec_dF(_sec_dF) 
    {
        if ((Point.size() != Value.size()) || (Value.size() != dF.size()) || (dF.size() != sec_dF.size()))
        {
            cerr<< "error:Input missmatch." <<endl;
		    exit(-1);
        }

        n = Point.size();
    }

    double operator()(double _x)
    {
        for (int i = 0; i < n; i++)
        {
            if (Point[i] == _x)
                return Value[i]; 
        }

        cerr<< "error: Invalid input." <<endl;
		exit(-1);

        return 0;
    }

    double diff(double _x)
    {
        for (int i = 0; i < n; i++)
        {
            if (Point[i] == _x)
                return dF[i]; 
        }

        cerr<< "error: Invalid input." <<endl;
		exit(-1);

        return 0;
    }

    double diff2(double _x)
    {
        for (int i = 0; i < n; i++)
        {
            if (Point[i] == _x)
                return sec_dF[i]; 
        }

        cerr<< "error: Invalid input." <<endl;
		exit(-1);

        return 0;
    }
};



class curve_spline : public spline
{
private:
    const double interval = 0.01;
    vector<vector<double> > Point, Fit_Point; 
    vector<double> cc_length; 
    int n, d, choice, order; 
public:
    curve_spline(vector<vector<double> > _Point, int _choice, int _order): Point(_Point), choice(_choice), order(_order)
    {
        if ((choice != 1) && (choice != 2))
	    {
		    cerr<< "error: Invalid choice " <<endl;
		    exit(-1);
	    }
        if ((order != 2) && (order != 3))
	    {
		    cerr<< "error:Invalid order " <<endl;
		    exit(-1);
	    }

        n = Point.size();
        d = Point[0].size();

        for (int i = 0; i < n; i++)
		{
            if (Point[i].size() != d)
            {
                cerr<< "error: Mismatch of sample points' dimension." <<endl;
		        exit(-1);
            }
		}
    }

    double norm_2(vector<double> x, vector<double> y)
    {
        double dist = 0.0;
        for (int i = 0; i < d; i++)
		{
            dist += (x[i]-y[i])*(x[i]-y[i]);
		}

        return sqrt(dist);
    }

    void cal()
    {
        cc_length.push_back(0.0);
        for (int i = 1; i < n; i++)
		{
            cc_length.push_back(cc_length[i-1] + norm_2(Point[i], Point[i-1]));
		}

        vector<double> ZERO(n);
        for (int i = 0; i < d; i++)
		{
            vector<double> temp_p, temp_fitp;
            for (int j = 0; j < n; j++)
		    {
                temp_p.push_back(Point[j][i]);
		    }
            Point_function temp_f(cc_length, temp_p, ZERO, ZERO);
            if (choice == 1)
            {
                Bspline BSI(temp_f,cc_length, 3, order);
                BSI.cal();
                double length = 0;
                while (length <= cc_length[n-1])
                {
                    temp_fitp.push_back(BSI(length));
                    length = length + interval;
                }
            }
            if (choice == 2)
            {
                PP_Form ppF(temp_f, cc_length, 3, order);
                ppF.cal();
                double length = 0;
                while (length <= cc_length[n-1])
                {
                    temp_fitp.push_back(ppF(length));
                    length = length + interval;
                }
            }
            Fit_Point.push_back(temp_fitp);
		}
    }

    vector<vector<double> > Get_Point()
    {
        return Fit_Point;
    }
};

#endif